
from math import cos, sin
import rospy
import sys, os
from cleaningrobot_gazebo.srv import bboxPerFrame, bboxPerFrameRequest

sys.path.append(os.path.dirname(__file__))
from _trackbox import *
pub_box = None
get_box = None

def pubbox(event):

    req = bboxPerFrameRequest()
    req.msg = 'bbox'

    try:
        res = get_box.call(req)

        tbox = trackbox()
        tbox.header.frame_id = 'world'
        tbox.header.stamp = rospy.Time.now()
        tbox.box_num = res.box_num

        rot_trans = lambda th, x, y: [cos(th)*x[0]-sin(th)*x[1] + y[0], sin(th)*x[0]+cos(th)*x[1] + y[1], x[2]]
        for i in range(res.box_num):
            ebox = res.bboxes[i]
            
            l, w, h = ebox.xyz
            z = ebox.pos[2]
            tr = [ebox.pos[0], ebox.pos[1], 0]
            yaw = ebox.yaw
            tbox.x1.extend(rot_trans(yaw, [-l/2, -w/2, z], tr))
            tbox.x2.extend(rot_trans(yaw, [+l/2, -w/2, z], tr))
            tbox.x3.extend(rot_trans(yaw, [+l/2, +w/2, z], tr))
            tbox.x4.extend(rot_trans(yaw, [-l/2, +w/2, z], tr))

            tbox.y1.extend(rot_trans(yaw, [-l/2, -w/2, z+h], tr))
            tbox.y2.extend(rot_trans(yaw, [+l/2, -w/2, z+h], tr))
            tbox.y3.extend(rot_trans(yaw, [+l/2, +w/2, z+h], tr))
            tbox.y4.extend(rot_trans(yaw, [-l/2, +w/2, z+h], tr))

        pub_box.publish(tbox)
    except:
        rospy.logwarn("service is not available")

if __name__ == "__main__":

    rospy.init_node("pubbox")

    pub_box = rospy.Publisher('/track_box', trackbox, queue_size=10)
    get_box = rospy.ServiceProxy('/getbbox', bboxPerFrame)
    get_box.wait_for_service()

    rospy.Timer(rospy.Duration(0.1), pubbox)
    rospy.spin()